

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <Eigen/Eigen>


mavros_msgs::State current_state;
Eigen::Vector3d current_pose(0,0,0);

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void pose_cb(geometry_msgs::PoseStamped msg)
{
    current_pose<<msg.pose.position.x,msg.pose.position.y,msg.pose.position.z;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard");
    ros::NodeHandle nh("~");

    int AGENT_ID=0;
    nh.getParam("agent_id",AGENT_ID);

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 1, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");

    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 1, pose_cb);


    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(30.0);

    // wait for FCU connection
    while (ros::ok() && !current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 0.8;

    // send a few setpoints before starting
    for (int i = 100; ros::ok() && i > 0; --i)
    {
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    bool takeoff_flag=false;
    while(ros::ok())
    {

        /* 向飞机发送请求进入offboard模式,飞机起飞之后，作往复直线运动。 */
        if( current_state.mode != "OFFBOARD")
            set_mode_client.call(offb_set_mode);
        if( !current_state.armed)
            arming_client.call(arm_cmd);

        if(!takeoff_flag)
        {
            if(current_pose(2)<1.8)
            {
                ROS_INFO_THROTTLE(1,"take off--------");
            }
            else{
                pose.pose.position.x=1.0;
                takeoff_flag=true;
            }
        }

        // if(current_pose(0)>0.8)
        // {
        //     pose.pose.position.x=-1;
        // }
        // if(current_pose(0)<-0.8)
        // {
        //     pose.pose.position.x=1;
        // }
        local_pos_pub.publish(pose);
        ROS_INFO_THROTTLE(1,"agent %d fly to target %f, %f, %f current_pose:%f %f %f------",AGENT_ID,pose.pose.position.x,
        pose.pose.position.y,pose.pose.position.z,current_pose(0),current_pose(1),current_pose(2));  
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}